namespace cs5643.rigidbody
{

    
    using Tao.OpenGl;
    using System;

    /** 
     * Spring force between one body and a proxy point. 
     * 
     * Requires modification to support mouse-spring forces between picked
     * non-center-of-mass positions on a rigid body.
     * 
     * @author Doug James, March 2007.
     */
    public class SpringForcePoint2Body : Force
    {
        RigidBody R;
        Pnt2D x;
        RigidBodySystem S;
        Block B;

        public SpringForcePoint2Body(RigidBody R, Pnt2D x, RigidBodySystem S)
        {
            if (R == null ) throw new NullReferenceException("R=" + R + ", x=" + x);

            this.R = R;
            this.x = new Pnt2D(x);
            this.S = S;
        }

        public SpringForcePoint2Body(RigidBody R, Block B, Pnt2D x,
           RigidBodySystem S)
        {
            if (R == null ) throw new Exception("R=" + R + ", x=" + x);
            this.B = B;
            this.R = R;
            this.x = new Pnt2D(x);
            this.S = S;
        }

        public void updatePoint(Pnt2D xNew)
        {
            x = xNew;
        }

        public void applyForce()
        {
            /// HACK: APPLY SPRING FORCE TO CENTER OF MASS:
            /// TODO: SUPPORT FORCES ON PICKED POSITIONS
            Pnt2D v;
            if (B == null)
            {
                v = Pnt2D.Sub(x , R.getPosition());
            }
            else
            {
                Pnt2D relCon = new Pnt2D(B.p);
                R.do_transformB2W(ref relCon);
                v = Pnt2D.Sub(x , relCon);
            }
            
            double L = v.Length;
            v.Normalize();
            //Pnt2D vel = R.getVelocityLinear();//fragile ref
            //double vn = vel.dot(v);
            double k = Constants.STIFFNESS_STRETCH_DENSITY * R.getMass();
            v.Times( k * L); //- 0.2*k*vn );//force

            double cross = 0;
            if (B != null)
            {
                Pnt2D relCon = new Pnt2D(B.p);
                R.do_transformB2W(ref relCon);
                relCon.Sub(R.getPosition());
                cross = relCon.x * v.y - relCon.y * v.x;
            }

            R.applyWrenchW(v, cross);

            /// DAMP RIGIDBODY MOTION
            v = R.v;
            v .Times( -0.2 * k);
            if (B != null)
            {
                Pnt2D relCon = new Pnt2D(B.p);
                R.do_transformB2W(ref relCon);
                relCon.Sub(R.getPosition());
                cross = relCon.x * v.y - relCon.y * v.x;
            }
            R.applyWrenchW(v, cross);
        }

        public void display()
        {
            /// DRAW A LINE:
            Gl.glColor3d(0, 1, 0);
            Gl.glBegin(Gl.GL_LINES);
            Pnt2D c = R.getPosition();
            if (B != null)
            {
                c = new Pnt2D(B.p);
                R.do_transformB2W(ref c);
            }
            Gl.glVertex3d((float)c.x, (float)c.y,0);
            Gl.glVertex3d((float)x.x, (float)x.y,0);
            Gl.glEnd();
        }

        public RigidBodySystem getSystem() { return S; }
    }
}